/**
  ******************************************************************************
  * File Name          : urm04_driver.cpp
  * Description       : urm04_driver program body
  ******************************************************************************
  *
  * Copyright (c) 2019 HopeMotion Co., Ltd. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of HopeMotion nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for HopeMotion.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY HOPEMOTION AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL HOPEMOTION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include <urm04_driver/urm04_driver.h>

std::string _port;
serial::Serial ser;
int _dev_num = 0;

int step = 0;
int r_step = 0;
int idx = 0;

unsigned char txd[TXD_MAX]; 
unsigned char rxd[RXD_MAX];

std::string id_s[SONAR_MAX] = {"1","2","3","4","5","6","7","8"};
unsigned char id_n[SONAR_MAX] = {0x11,0x12,0x13,0x14,0x15,0x16,0x17,0x18};
float dis[SONAR_MAX] = {65535,65535,65535,65535,65535,65535,65535,65535};                     

/* Private function prototypes -----------------------------------------------*/

/**
  * @brief  通信数据校验和
  * @param
  * @retval
  */
unsigned char checksum(unsigned char buf[TXD_MAX])
{
    unsigned int cs=0;
    for(int i = 0; i<TXD_MAX-1; i++)
    {
        cs+=buf[i];
    }
    return (unsigned char)cs;
}

/**
  * @brief
  * @param
  * @retval
  */
int tx_sig(unsigned char id,unsigned char cmd)
{
    txd[0] = 0x55; 
    txd[1] = 0xAA;
    txd[2] = id_n[id%SONAR_MAX];
    txd[3] = 0x00;
    txd[4] = cmd;
    txd[5] = checksum(txd);
    //printf("txd:[%02X %02X %02X %02X %02X %02X]\r\n",txd[0],txd[1],txd[2],txd[3],txd[4],txd[5]);
    ser.write(txd,TXD_MAX);
    return 0;
}

/**
  * @brief
  * @param
  * @retval
  */
int rx_sig(void)
{
    unsigned int tmp = 0;
    int index = 0;
    if (ser.available())
    {
        ser.read(rxd, RXD_MAX);
        //printf("rxd:[%02X %02X %02X %02X %02X %02X %02X %02X]\r\n",
        //          rxd[0],rxd[1],rxd[2],rxd[3],rxd[4],rxd[5],rxd[6],rxd[7]);
        index = rxd[2] - 0x11;
        if((rxd[0]==0x55)&&(rxd[1]==0xAA))
        {
            tmp = rxd[5];
            tmp <<= 8;
            tmp += rxd[6];
            dis[index] = tmp;
            dis[index] /=100;
            //printf("dis%d:[%0.2f]\r\n",index, dis[index]);
            return 0;
        }
    }
    return -1;
}


/**
  * @brief  驱动主函数
  * @param
  * @retval
  */
int main(int argc, char**argv)
{
    ros::init(argc, argv, "urm04_driver");
    ros::NodeHandle nh;
    ros::Rate loop_rate(30);

    nh.getParam("/urm04_driver/dev_num", _dev_num);
    if((_dev_num>SONAR_MAX)||(_dev_num<0))
    {
        _dev_num = 0;  
    }
    nh.getParam("/urm04_driver/port", _port);

    sensor_msgs::Range _range[SONAR_MAX];
    //sensor_msgs::Temperature _temp[SONAR_MAX];

    ros::Publisher sonar_rpub[SONAR_MAX];
    //ros::Publisher sonar_tpub[SONAR_MAX];

    int i = 0;
  
    //发布超声波话题
    for(i=0; i<_dev_num; i++)
    {
        sonar_rpub[i] = nh.advertise<sensor_msgs::Range>("/sonar/"+id_s[i]+"/range", 5);
        //sonar_tpub[i] = nh.advertise<sensor_msgs::Temperature>("/sonar/"+id_s[i]+"/temperature", 5);
    }
    try 
    { 
    //设置串口属性，并打开串口 
        ser.setPort(_port.c_str()); 
        ser.setBaudrate(19200); 
        serial::Timeout to = serial::Timeout::simpleTimeout(5); 
        ser.setTimeout(to); 
        ser.open(); 
    } 
    catch (serial::IOException& e) 
    { 
        ROS_ERROR_STREAM("Unable to open port "); 
        return -1; 
    } 


    while (ros::ok())
    {
        switch(step)
        {
            case 0: //数据测量
                if(idx<_dev_num)
                {
                    tx_sig(idx,1);
                    idx++;
                    if(idx>=_dev_num)
                    {
                        idx = 0;
                        step = 1;
                    }
                }
                break;
            case 1: //读取距离
                switch(r_step)
                {
                    case 0:
                        tx_sig(idx,2);
                        r_step = 1;
                        break;
                    case 1:
                        rx_sig();
                        r_step = 0;
                        idx++;
                        if(idx>=_dev_num)
                        {
                            idx = 0;
                            step = 0;
                        }
                        break;
                    default:
                        idx = 0;
                        step = 0;
                        r_step = 0;
                        break;
                }
                break;
            default:
                step = 0;
                break;
        }

        for(i=0; i<_dev_num; i++)
        {
            _range[i].header.frame_id = id_s[i];
            _range[i].radiation_type = 0;
            _range[i].field_of_view = 0.2;
            _range[i].min_range = 0.04;
            _range[i].max_range = 5;
            //printf("%d:%0.3f\r\n",i,dis[i]);
            _range[i].range = dis[i];
            sonar_rpub[i].publish(_range[i]);
        }

        ros::spinOnce();
        loop_rate.sleep();
    }

    if(ser.isOpen())
    {
        ser.close();
    }
  
    return 0;
}
